﻿#include "calibration.h"
#include <QDebug>

Calibration::Calibration()
{

}

void Calibration::circleLeastFit(const std::vector<cv::Point2d> points, cv::Point2d &center, double &radius)
{
    radius = 0.0f;
    double sum_x = 0.0f, sum_y = 0.0f;
    double sum_x2 = 0.0f, sum_y2 = 0.0f;
    double sum_x3 = 0.0f, sum_y3 = 0.0f;
    double sum_xy = 0.0f, sum_x1y2 = 0.0f, sum_x2y1 = 0.0f;
    int N = points.size();
    for (int i = 0; i < N; i++)
    {
        double x = points[i].x;
        double y = points[i].y;
        double x2 = x * x;
        double y2 = y * y;
        sum_x += x;
        sum_y += y;
        sum_x2 += x2;
        sum_y2 += y2;
        sum_x3 += x2 * x;
        sum_y3 += y2 * y;
        sum_xy += x * y;
        sum_x1y2 += x * y2;
        sum_x2y1 += x2 * y;
    }
    double C, D, E, G, H;
    double a, b, c;
    C = N * sum_x2 - sum_x * sum_x;
    D = N * sum_xy - sum_x * sum_y;
    E = N * sum_x3 + N * sum_x1y2 - (sum_x2 + sum_y2) * sum_x;
    G = N * sum_y2 - sum_y * sum_y;
    H = N * sum_x2y1 + N * sum_y3 - (sum_x2 + sum_y2) * sum_y;
    a = (H * D - E * G) / (C * G - D * D);
    b = (H * C - E * D) / (D * D - G * C);
    c = -(a * sum_x + b * sum_y + sum_x2 + sum_y2) / N;
    center.x = a / (-2);
    center.y = b / (-2);
    radius = sqrt(a * a + b * b - 4 * c) / 2;

    mark_center = center;

}

cv::Point2d Calibration::getMarkCenter(){
        return mark_center;
    }


cv::Mat Calibration::getAffineTransform64f(std::string name,
                                           int index,
                                           const std::vector<cv::Point2f> src,
                                           const std::vector<cv::Point2f> dst)
{
    cv::Mat M(2, 3, CV_64F), X(6, 1, CV_64F, M.data);

    int SIZE = src.size() * 2;

    double *a = new double[SIZE*6];
    double *b = new double[SIZE];

    cv::Mat A(SIZE, 6, CV_64F, a), B(SIZE, 1, CV_64F, b);

    for( int i = 0; i < src.size(); i++ )
    {
        int j = i*12;
        int k = i*12+6;
        a[j] = a[k+3] = src[i].x;
        a[j+1] = a[k+4] = src[i].y;
        a[j+2] = a[k+5] = 1;
        a[j+3] = a[j+4] = a[j+5] = 0;
        a[k] = a[k+1] = a[k+2] = 0;
        b[i*2] = dst[i].x;
        b[i*2+1] = dst[i].y;
    }

    cv::solve( A, B, X ,cv::DECOMP_SVD);

    if("tray" == name)
    {
        //tray_warp_mat = M.clone();
    }
    else if("station" == name)
    {
        //station_id = index;

        //current_wrap_mat
        //current_station_warp_mat = M.clone();

        std::vector<double> warp_vector;

        warp_vector.push_back(M.at<double>(0,0));
        warp_vector.push_back(M.at<double>(0,1));
        warp_vector.push_back(M.at<double>(0,2));
        warp_vector.push_back(M.at<double>(1,0));
        warp_vector.push_back(M.at<double>(1,1));
        warp_vector.push_back(M.at<double>(1,2));

        //station_wrap_mat_[station_id] = warp_vector;
    }

    //save_yaml();

    qDebug() << "M data " << M.at<double>(0,0) << " "
             << M.at<double>(0,1) << " "
             << M.at<double>(0,2) << " "
             << M.at<double>(1,0) << " "
             << M.at<double>(1,1) << " "
             << M.at<double>(1,2);


    return M;
}


bool Calibration::leastSquaresFitting(const std::vector<cv::Point2d>& points){

        double X1 = 0;
        double Y1 = 0;
        double X2 = 0;
        double Y2 = 0;
        double X3 = 0;
        double Y3 = 0;
        double X1Y1 = 0;
        double X1Y2 = 0;
        double X2Y1 = 0;

        for (int i = 0; i < points.size(); ++i)
        {
            X1 = X1 + points[i].x;
            Y1 = Y1 + points[i].y;
            X2 = X2 + points[i].x * points[i].x;
            Y2 = Y2 + points[i].y * points[i].y;
            X3 = X3 + points[i].x * points[i].x * points[i].x;
            Y3 = Y3 + points[i].y * points[i].y * points[i].y;
            X1Y1 = X1Y1 + points[i].x * points[i].y;
            X1Y2 = X1Y2 + points[i].x * points[i].y * points[i].y;
            X2Y1 = X2Y1 + points[i].x * points[i].x * points[i].y;
        }

        double C = 0, D = 0, E = 0, G = 0, H = 0, N = 0;
        double a = 0, b = 0, c = 0;
        N = points.size();
        C = N*X2 - X1*X1;
        D = N*X1Y1 - X1*Y1;
        E = N*X3 + N*X1Y2 - (X2+Y2)*X1;
        G = N*Y2 - Y1*Y1;
        H = N*X2Y1 + N*Y3 - (X2+Y2)*Y1;
        a = (H*D-E*G)/(C*G-D*D);
        b = (H*C-E*D)/(D*D-G*C);
        c = -(a*X1 + b*Y1 + X2 + Y2)/N;

        double LeastSquaresFittingCenterX = a/(-2);
        double LeastSquaresFittingCenterY = b/(-2);
        //double LeastSquaresFittingRadius = qSqrt(a*a+b*b-4*c)/2;

        deltaX = LeastSquaresFittingCenterX - points[1].x;
        deltaY = LeastSquaresFittingCenterY - points[1].y;

        return true;

}

cv::Mat Calibration::LocalAffineEstimate(const std::vector<cv::Point2d>& shape1,
                            const std::vector<cv::Point2d>& shape2,
                            bool fullAfine){

    cv::Mat out(2, 3, CV_32F);
    int siz = 2 * (int)shape1.size();

    if (fullAfine)
    {
        cv::Mat matM(siz, 6, CV_32F);
        cv::Mat matP(siz, 1, CV_32F);
        int contPt = 0;
        for (int ii = 0; ii<siz; ii++)
        {
            cv::Mat therow = cv::Mat::zeros(1, 6, CV_32F);
            if (ii % 2 == 0)
            {
                therow.at<float>(0, 0) = shape1[contPt].x;
                therow.at<float>(0, 1) = shape1[contPt].y;
                therow.at<float>(0, 2) = 1;
                therow.row(0).copyTo(matM.row(ii));
                matP.at<float>(ii, 0) = shape2[contPt].x;
            }
            else
            {
                therow.at<float>(0, 3) = shape1[contPt].x;
                therow.at<float>(0, 4) = shape1[contPt].y;
                therow.at<float>(0, 5) = 1;
                therow.row(0).copyTo(matM.row(ii));
                matP.at<float>(ii, 0) = shape2[contPt].y;
                contPt++;
            }
        }
        cv::Mat sol;
        solve(matM, matP, sol, cv::DECOMP_SVD);
        out = sol.reshape(0, 2);
    }
    else
    {
        cv::Mat matM(siz, 4, CV_32F);
        cv::Mat matP(siz, 1, CV_32F);
        int contPt = 0;
        for (int ii = 0; ii<siz; ii++)
        {
            cv::Mat therow = cv::Mat::zeros(1, 4, CV_32F);
            if (ii % 2 == 0)
            {
                therow.at<float>(0, 0) = shape1[contPt].x;
                therow.at<float>(0, 1) = shape1[contPt].y;
                therow.at<float>(0, 2) = 1;
                therow.row(0).copyTo(matM.row(ii));
                matP.at<float>(ii, 0) = shape2[contPt].x;
            }
            else
            {
                therow.at<float>(0, 0) = -shape1[contPt].y;
                therow.at<float>(0, 1) = shape1[contPt].x;
                therow.at<float>(0, 3) = 1;
                therow.row(0).copyTo(matM.row(ii));
                matP.at<float>(ii, 0) = shape2[contPt].y;
                contPt++;
            }
        }
        cv::Mat sol;
        cv::solve(matM, matP, sol, cv::DECOMP_SVD);
        out.at<float>(0, 0) = sol.at<float>(0, 0);
        out.at<float>(0, 1) = sol.at<float>(1, 0);
        out.at<float>(0, 2) = sol.at<float>(2, 0);
        out.at<float>(1, 0) = -sol.at<float>(1, 0);
        out.at<float>(1, 1) = sol.at<float>(0, 0);
        out.at<float>(1, 2) = sol.at<float>(3, 0);
    }
    return out;

}

void Calibration::calculateTransformMatrix(const std::vector<cv::Point2d>& points_camera,
                              const std::vector<cv::Point2d>& points_robot){

    cv::Mat  affine;
    cv::estimateRigidTransform(points_camera, points_robot, true).convertTo(affine, CV_32F);
    if (affine.empty())
    {
        affine = LocalAffineEstimate(points_camera, points_robot, true);
    }

    A = affine.at<float>(0, 0);
    B = affine.at<float>(0, 1);
    C = affine.at<float>(0, 2);
    D = affine.at<float>(1, 0);
    E = affine.at<float>(1, 1);
    F = affine.at<float>(1, 2);

    double sumX = 0, sumY = 0;
    for (int i = 0; i < points_camera.size(); ++i)
    {
        cv::Point2d pt;
        pt.x = A * points_camera[i].x + B * points_camera[i].y + C;
        pt.y = D * points_camera[i].x + E * points_camera[i].y + F;
        //saveCalibPointsRobot.append(QJsonValue(pt.x + '\n'));
        //saveCalibPointsRobot.append(QJsonValue(pt.y + '\n'));
        sumX += pow(points_robot[i].x - pt.x, 2);
        sumY += pow(points_robot[i].y - pt.y, 2);
    }
    double rmsX = sqrt(sumX / points_camera.size());
    double rmsY = sqrt(sumY / points_camera.size());
    //saveCalibrationMatrix.insert("rmsX",QJsonValue(rmsX));
    //saveCalibrationMatrix.insert("rmsY",QJsonValue(rmsY));

    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz") << "A:" << A << "\tB:" << B << "\tC:" << C << "\tD:" << D << "\tE:" << E << "\tF:" << F;
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz") << "rmsX:" << rmsX << "\trmsY:" << rmsY;
}


bool Calibration::add_station_transform_matrix(QString station_id, cv::Mat wrap_mat){

    if(stations_transform_matrix_.find(station_id) == stations_transform_matrix_.end()){
        stations_transform_matrix_.emplace(station_id,wrap_mat);
    }
    else{
        stations_transform_matrix_.at(station_id) = wrap_mat;
    }

    return true;
}

bool Calibration::remove_station_transform_matrix(QString station_id, cv::Mat wrap_mat){

    if(stations_transform_matrix_.find(station_id) == stations_transform_matrix_.end()){
        return true;
    }
    else{
        stations_transform_matrix_.erase(station_id);
    }

    return true;
}

std::map<QString, cv::Mat> Calibration::get_station_transform_matrix(){

    return stations_transform_matrix_;
}

bool Calibration::add_station_reference_angle(QString station_id, float angle){

    if(stations_reference_angle.find(station_id) == stations_reference_angle.end()){
        stations_reference_angle.emplace(station_id,angle);
    }
    else{
        stations_reference_angle.at(station_id) = angle;
    }

    return true;
}
bool Calibration::remove_station_reference_angle(QString station_id, float angle){


    if(stations_reference_angle.find(station_id) == stations_reference_angle.end()){
        return true;
    }
    else{
        stations_reference_angle.erase(station_id);
    }

    return true;
}
std::map<QString, float> Calibration::get_station_reference_angle(){
    return stations_reference_angle;
}

bool Calibration::correct_station_pose(QString station_id,
                                       cv::Point2d qr_point,
                                       float qr_angle,
                                       RobotPoseXYZ reference_pose,
                                       RobotPoseXYZ target_pose,
                                       RobotPoseXYZ& corrected_target_pose){

    //transfer degree to rad
    reference_pose.rx *= M_PI/180.0;
    reference_pose.ry *= M_PI/180.0;
    reference_pose.rz *= M_PI/180.0;

    target_pose.rx *= M_PI/180.0;
    target_pose.ry *= M_PI/180.0;
    target_pose.rz *= M_PI/180.0;

    qDebug() << "reference_pose.rx " << reference_pose.rx;
    qDebug() << "reference_pose.ry " << reference_pose.ry;
    qDebug() << "reference_pose.rz " << reference_pose.rz;


    //correct xy
    std::vector<cv::Point2f> pixel_station;
    pixel_station.push_back(qr_point);

    cv::Mat warp_mat(2,3,CV_32FC1);
    if(stations_transform_matrix_.find(station_id) == stations_transform_matrix_.end()){
        return false;
    }
    warp_mat = stations_transform_matrix_.at(station_id);
    
    std::vector<cv::Point2f> outcome;
    cv::transform(pixel_station, outcome, warp_mat);

    //correct angle;
    if(stations_reference_angle.find(station_id) == stations_reference_angle.end()){
        return false;
    }

    //degree or rad?
    float angle_offset = qr_angle - stations_reference_angle.at(station_id);
    float angle_offset_rad = angle_offset * M_PI / 180.0;

    qDebug() << "qr_angle " << qr_angle;
    qDebug() << "stations_reference_angle.at(station_id) " << stations_reference_angle.at(station_id);
    qDebug() << "angle_offset " << angle_offset;
    qDebug() << "angle_offset_rad " << angle_offset_rad;
    qDebug() << "reference_pose.rz + angle_offset_rad " << reference_pose.rz + angle_offset_rad;


    transform::Rigid3d transform_corrected_reference_pose(transform::Rigid3d::Vector(outcome[0].x,
                                                                                     outcome[0].y,
                                                                                     reference_pose.z),
                                                          transform::RollPitchYaw(reference_pose.rx,
                                                                                  reference_pose.ry,
                                                                                  reference_pose.rz + angle_offset_rad));


//    Eigen::Vector3d corrected_eularAngle = transform_corrected_reference_pose.rotation().matrix().eulerAngles(0,1,2);

//    qDebug() << "corrected_eularAngle "
//             << corrected_eularAngle[0] << " "
//             << corrected_eularAngle[1] << " "
//             << corrected_eularAngle[2];


//    Eigen::Vector3d corrected_eularAngle_1 = transform_corrected_reference_pose.rotation().matrix().eulerAngles(2,1,0);

//    qDebug() << "corrected_eularAngle_1 "
//             << corrected_eularAngle_1[2] << " "
//             << corrected_eularAngle_1[1] << " "
//             << corrected_eularAngle_1[0];

//    Eigen::Vector3d corrected_eularAngle_2 = transform_corrected_reference_pose.rotation().matrix().eulerAngles(0,2,1);

//    qDebug() << "corrected_eularAngle_2 "
//             << corrected_eularAngle_2[0] << " "
//             << corrected_eularAngle_2[2] << " "
//             << corrected_eularAngle_2[1];

//    Eigen::Vector3d corrected_eularAngle_3 = transform_corrected_reference_pose.rotation().matrix().eulerAngles(2,0,1);

//    qDebug() << "corrected_eularAngle_3 "
//             << corrected_eularAngle_3[1] << " "
//             << corrected_eularAngle_3[2] << " "
//             << corrected_eularAngle_3[0];

    Eigen::Vector3d corrected_eularAngle_4 = transform_corrected_reference_pose.rotation().matrix().eulerAngles(1,2,0);

    qDebug() << "corrected_eularAngle_4 "
             << corrected_eularAngle_4[2] << " "
             << corrected_eularAngle_4[0] << " "
             << corrected_eularAngle_4[1];

//    Eigen::Vector3d corrected_eularAngle_5 = transform_corrected_reference_pose.rotation().matrix().eulerAngles(1,0,2);

//    qDebug() << "corrected_eularAngle_5 "
//             << corrected_eularAngle_5[1] << " "
//             << corrected_eularAngle_5[0] << " "
//             << corrected_eularAngle_5[2];


    //get reference pose;
    transform::Rigid3d transform_reference_pose(
                transform::Rigid3d::Vector(reference_pose.x,
                                           reference_pose.y,
                                           reference_pose.z),
                transform::RollPitchYaw(reference_pose.rx,
                                        reference_pose.ry,
                                        reference_pose.rz));
    
    //get target pose;
    transform::Rigid3d transform_target_pose(
                transform::Rigid3d::Vector(target_pose.x,
                                           target_pose.y,
                                           target_pose.z),
                transform::RollPitchYaw(target_pose.rx,
                                        target_pose.ry,
                                        target_pose.rz));
    
    transform::Rigid3d relative_matrix = transform_reference_pose.inverse() * transform_target_pose;

    qDebug() << "relative_matrix "
             << relative_matrix.translation()[0] << " "
             << relative_matrix.translation()[1] << " "
             << relative_matrix.translation()[2];

    Eigen::Vector3d relative_matrix_eularAngle = relative_matrix.rotation().matrix().eulerAngles(1,0,2);

    qDebug() << "relative_matrix_eularAngle "
             << relative_matrix_eularAngle[0] << " "
             << relative_matrix_eularAngle[1] << " "
             << relative_matrix_eularAngle[2];

    transform::Rigid3d transform_corrected_target_pose = transform_corrected_reference_pose * relative_matrix;

    corrected_target_pose.x = transform_corrected_target_pose.translation()[0];
    corrected_target_pose.y = transform_corrected_target_pose.translation()[1];
    corrected_target_pose.z = transform_corrected_target_pose.translation()[2];

    Eigen::Vector3d eularAngle = transform_corrected_target_pose.rotation().matrix().eulerAngles(1,2,0);

    corrected_target_pose.rx = eularAngle[2] * 180.0 / M_PI;
    corrected_target_pose.ry = eularAngle[0] * 180.0 / M_PI;
    corrected_target_pose.rz = eularAngle[1] * 180.0 / M_PI;
    //get fixed transform

    qDebug() << "corrected_target_pose "
             << corrected_target_pose.rx << " "
             << corrected_target_pose.ry << " "
             << corrected_target_pose.rz;

    return false;
}


bool Calibration::correct_station_pose(QString station_id,
                          cv::Point2d qr_point,
                          float qr_angle,
                          RobotPoseXYZ target_pose,
                          RobotPoseXYZ& corrected_target_pose){

    if(stations_reference_pose.find(station_id) == stations_reference_pose.end()){
        return false;
    }

    RobotPoseXYZ reference_pose = stations_reference_pose.at(station_id);

    //transfer degree to rad
    reference_pose.rx *= M_PI/180.0;
    reference_pose.ry *= M_PI/180.0;
    reference_pose.rz *= M_PI/180.0;

    target_pose.rx *= M_PI/180.0;
    target_pose.ry *= M_PI/180.0;
    target_pose.rz *= M_PI/180.0;


    tf::Vector3 reference_pose_t(reference_pose.x,reference_pose.y,reference_pose.z);
    tf::Quaternion reference_pose_q;
    reference_pose_q.setRPY(reference_pose.rx,reference_pose.ry,reference_pose.rz);
    tf::Transform reference_pose_tf(reference_pose_q, reference_pose_t);


    tf::Vector3 target_pose_t(target_pose.x, target_pose.y, target_pose.z);
    tf::Quaternion target_pose_q;
    target_pose_q.setRPY(target_pose.rx, target_pose.ry, target_pose.rz);
    tf::Transform target_pose_tf(target_pose_q, target_pose_t);

    tf::Transform relative_transform = reference_pose_tf.inverse() * target_pose_tf;


//    qDebug() << "reference_pose.x " << reference_pose.x;
//    qDebug() << "reference_pose.y " << reference_pose.y;
//    qDebug() << "reference_pose.z " << reference_pose.z;
//    qDebug() << "reference_pose.rx " << reference_pose.rx;
//    qDebug() << "reference_pose.ry " << reference_pose.ry;
//    qDebug() << "reference_pose.rz " << reference_pose.rz;


//    qDebug() << "target_pose.x " << target_pose.x;
//    qDebug() << "target_pose.y " << target_pose.y;
//    qDebug() << "target_pose.z " << target_pose.z;
//    qDebug() << "target_pose.rx " << target_pose.rx;
//    qDebug() << "target_pose.ry " << target_pose.ry;
//    qDebug() << "target_pose.rz " << target_pose.rz;

    //correct xy
    std::vector<cv::Point2f> pixel_station;
    pixel_station.push_back(qr_point);

    //qDebug() << "qr_point.x " << qr_point.x;
    //qDebug() << "qr_point.y " << qr_point.y;

    cv::Mat warp_mat(2,3,CV_32FC1);
    if(stations_transform_matrix_.find(station_id) == stations_transform_matrix_.end()){
        return false;
    }
    warp_mat = stations_transform_matrix_.at(station_id);

    std::vector<cv::Point2f> outcome;
    cv::transform(pixel_station, outcome, warp_mat);

    //correct angle;
    if(stations_reference_angle.find(station_id) == stations_reference_angle.end()){
        return false;
    }

    //degree or rad?
    float angle_offset = stations_reference_angle.at(station_id) - qr_angle;
    float angle_offset_rad = angle_offset * M_PI / 180.0;

    //qDebug() << "qr_angle " << qr_angle;
    //qDebug() << "stations_reference_angle.at(station_id) " << stations_reference_angle.at(station_id);
    //qDebug() << "angle_offset " << angle_offset;
    //qDebug() << "angle_offset_rad " << angle_offset_rad;
    //qDebug() << "reference_pose.rz + angle_offset_rad " << reference_pose.rz + angle_offset_rad;



     //   qDebug() << "base_pose.x " << outcome[0].x;
     //   qDebug() << "base_pose.y " << outcome[0].y;
     //   qDebug() << "base_pose.z " << reference_pose.z;
     //   qDebug() << "base_pose.rx " << reference_pose.rx;
     //   qDebug() << "base_pose.ry " << reference_pose.ry;
     //   qDebug() << "base_pose.rz " << reference_pose.rz;
     //   qDebug() << "angle_offset_rad " << angle_offset_rad;

    tf::Vector3 base_pose_t(outcome[0].x, outcome[0].y, reference_pose.z);
    tf::Quaternion base_pose_q;
    base_pose_q.setRPY(reference_pose.rx,
                  reference_pose.ry,
                  reference_pose.rz + angle_offset_rad);

    tf::Transform base_pose(base_pose_q, base_pose_t);




    tf::Transform corrent_pose = base_pose * relative_transform;


    corrected_target_pose.x = corrent_pose.getOrigin().x();
    corrected_target_pose.y = corrent_pose.getOrigin().y();
    corrected_target_pose.z = corrent_pose.getOrigin().z();

    double roll, pitch, yaw;

    corrent_pose.getBasis().getRPY(roll, pitch, yaw);

    corrected_target_pose.rx = roll * 180.0 / M_PI;
    corrected_target_pose.ry = pitch * 180.0 / M_PI;
    corrected_target_pose.rz = yaw * 180.0 / M_PI;

//    qDebug() << "x " << corrected_target_pose.x;
//    qDebug() << "y " << corrected_target_pose.y;
//    qDebug() << "z " << corrected_target_pose.z;
//    qDebug() << "rx " << corrected_target_pose.rx;
//    qDebug() << "ry " << corrected_target_pose.ry;
//    qDebug() << "rz " << corrected_target_pose.rz;

    return true;

}


bool Calibration::get_base_pose(QString station_id,
                   cv::Point2d qr_point,
                   float qr_angle,
                   RobotPoseXYZ& reference_pose,
                   RobotPoseXYZ& base_pose){


    qDebug() << "reference_pose x " << reference_pose.x;
    qDebug() << "reference_pose y " << reference_pose.y;
    qDebug() << "reference_pose z " << reference_pose.z;
    qDebug() << "reference_pose rx " << reference_pose.rx;
    qDebug() << "reference_pose ry " << reference_pose.ry;
    qDebug() << "reference_pose rz " << reference_pose.rz;


    //transfer degree to rad
    reference_pose.rx *= M_PI/180.0;
    reference_pose.ry *= M_PI/180.0;
    reference_pose.rz *= M_PI/180.0;


    //correct xy
    std::vector<cv::Point2f> pixel_station;
    pixel_station.push_back(qr_point);

    cv::Mat warp_mat(2,3,CV_32FC1);
    if(stations_transform_matrix_.find(station_id) == stations_transform_matrix_.end()){
        return false;
    }
    warp_mat = stations_transform_matrix_.at(station_id);

    std::vector<cv::Point2f> outcome;
    cv::transform(pixel_station, outcome, warp_mat);

    //correct angle;
    if(stations_reference_angle.find(station_id) == stations_reference_angle.end()){
        return false;
    }

    //degree or rad?
    float angle_offset = stations_reference_angle.at(station_id) - qr_angle;
    float angle_offset_rad = angle_offset * M_PI / 180.0;

    qDebug() << "qr_angle " << qr_angle;
    qDebug() << "stations_reference_angle.at(station_id) " << stations_reference_angle.at(station_id);
    qDebug() << "angle_offset " << angle_offset;
    qDebug() << "angle_offset_rad " << angle_offset_rad;
    qDebug() << "reference_pose.rz + angle_offset_rad " << reference_pose.rz + angle_offset_rad;


    tf::Vector3 base_pose_t(outcome[0].x, outcome[0].y, reference_pose.z);
    tf::Quaternion base_pose_q;
    base_pose_q.setRPY(reference_pose.rx,
                  reference_pose.ry,
                  reference_pose.rz + angle_offset_rad);

    tf::Transform base_pose_tf(base_pose_q, base_pose_t);

    base_pose.x = base_pose_tf.getOrigin().x();
    base_pose.y = base_pose_tf.getOrigin().y();
    base_pose.z = base_pose_tf.getOrigin().z();

    double roll, pitch, yaw;

    base_pose_tf.getBasis().getRPY(roll, pitch, yaw);

    base_pose.rx = roll * 180.0 / M_PI;
    base_pose.ry = pitch * 180.0 / M_PI;
    base_pose.rz = yaw * 180.0 / M_PI;

    qDebug() << "x " << base_pose.x;
    qDebug() << "y " << base_pose.y;
    qDebug() << "z " << base_pose.z;
    qDebug() << "rx " << base_pose.rx;
    qDebug() << "ry " << base_pose.ry;
    qDebug() << "rz " << base_pose.rz;

    return true;
}


bool Calibration::add_station_reference_pose(QString station_id, RobotPoseXYZ pose){
    if(stations_reference_pose.find(station_id) == stations_reference_pose.end()){
        stations_reference_pose.emplace(station_id, pose);
    }
    else{
        stations_reference_pose.at(station_id) = pose;
    }

    return true;

}

std::map<QString, RobotPoseXYZ> Calibration::get_station_reference_pose(){
    return stations_reference_pose;
}


bool Calibration::add_station_type(QString station_id, QString type){

    if(stations_type.find(station_id) == stations_type.end()){
        stations_type.emplace(station_id, type);
    }
    else{
        stations_type.at(station_id) = type;
    }

    return true;
}

std::map<QString, QString> Calibration::get_station_type(){

    return stations_type;
}

QString Calibration::get_station_type(QString station_id){

    if(stations_type.find(station_id) == stations_type.end()){
        return "";
    }
    else{
        return stations_type.at(station_id);
    }
}


bool Calibration::add_station_exposure_time(QString station_id, int value){
    if(stations_exposure_time.find(station_id) == stations_exposure_time.end()){
        stations_exposure_time.emplace(station_id,value);
    }
    else{
        stations_exposure_time.at(station_id) = value;
    }

    return true;
}


int Calibration::get_station_exposure_time(QString station_id){
    if(stations_exposure_time.find(station_id) == stations_exposure_time.end()){
        return 5000;
    }
    else{
        return stations_exposure_time.at(station_id);
    }
}

//用于简易标定
bool Calibration::refine_station_pose(RobotPoseXYZ origin_pose,
                         RobotPoseXYZ correct_origin_pose,
                         RobotPoseXYZ target_pose,
                         RobotPoseXYZ& corrected_target_pose){

    //transfer degree to rad
    origin_pose.rx *= M_PI/180.0;
    origin_pose.ry *= M_PI/180.0;
    origin_pose.rz *= M_PI/180.0;

    correct_origin_pose.rx *= M_PI/180.0;
    correct_origin_pose.ry *= M_PI/180.0;
    correct_origin_pose.rz *= M_PI/180.0;


    tf::Vector3 origin_pose_t(origin_pose.x,origin_pose.y,origin_pose.z);
    tf::Quaternion origin_pose_q;
    origin_pose_q.setRPY(origin_pose.rx,origin_pose.ry,origin_pose.rz);
    tf::Transform origin_pose_tf(origin_pose_q, origin_pose_t);


    tf::Vector3 correct_origin_pose_t(correct_origin_pose.x, correct_origin_pose.y, correct_origin_pose.z);
    tf::Quaternion correct_origin_pose_q;
    correct_origin_pose_q.setRPY(correct_origin_pose.rx, correct_origin_pose.ry, correct_origin_pose.rz);
    tf::Transform correct_origin_pose_tf(correct_origin_pose_q, correct_origin_pose_t);

    tf::Transform relative_transform = origin_pose_tf.inverse() * correct_origin_pose_tf;


//    qDebug() << "reference_pose.x " << reference_pose.x;
//    qDebug() << "reference_pose.y " << reference_pose.y;
//    qDebug() << "reference_pose.z " << reference_pose.z;
//    qDebug() << "reference_pose.rx " << reference_pose.rx;
//    qDebug() << "reference_pose.ry " << reference_pose.ry;
//    qDebug() << "reference_pose.rz " << reference_pose.rz;


//    qDebug() << "target_pose.x " << target_pose.x;
//    qDebug() << "target_pose.y " << target_pose.y;
//    qDebug() << "target_pose.z " << target_pose.z;
//    qDebug() << "target_pose.rx " << target_pose.rx;
//    qDebug() << "target_pose.ry " << target_pose.ry;
//    qDebug() << "target_pose.rz " << target_pose.rz;

    target_pose.rx *= M_PI/180.0;
    target_pose.ry *= M_PI/180.0;
    target_pose.rz *= M_PI/180.0;

    tf::Vector3 target_pose_t(target_pose.x,target_pose.y,target_pose.z);
    tf::Quaternion target_pose_q;
    target_pose_q.setRPY(target_pose.rx,target_pose.ry,target_pose.rz);
    tf::Transform target_pose_tf(target_pose_q, target_pose_t);

    tf::Transform corrent_pose = target_pose_tf * relative_transform;

    corrected_target_pose.x = corrent_pose.getOrigin().x();
    corrected_target_pose.y = corrent_pose.getOrigin().y();
    corrected_target_pose.z = corrent_pose.getOrigin().z();

    double roll, pitch, yaw;

    corrent_pose.getBasis().getRPY(roll, pitch, yaw);

    corrected_target_pose.rx = roll * 180.0 / M_PI;
    corrected_target_pose.ry = pitch * 180.0 / M_PI;
    corrected_target_pose.rz = yaw * 180.0 / M_PI;

//    qDebug() << "x " << corrected_target_pose.x;
//    qDebug() << "y " << corrected_target_pose.y;
//    qDebug() << "z " << corrected_target_pose.z;
//    qDebug() << "rx " << corrected_target_pose.rx;
//    qDebug() << "ry " << corrected_target_pose.ry;
//    qDebug() << "rz " << corrected_target_pose.rz;

    return true;

}
